% function [dQdt, u_opt] = proximity_quotient_dot_cutoff(sc, st, F, mu, W)

function [dQdt, u_opt,cutoff_abs,cutoff_rel] = proximity_quotient_dot_cutoff(sc, st, F, mu, W)

pp = sc(1);
ff = sc(2);
gg = sc(3);
hh = sc(4);
kk = sc(5);
LL = sc(6);

pp_t = st(1);
ff_t = st(2);
gg_t = st(3);
hh_t = st(4);
kk_t = st(5);

[aa,ecc,~,~,~,nu] = lowThrustMee2Coe(pp,ff,gg,hh,kk,LL);

q   = 1 + ff*cos(LL) + gg*sin(LL);
s2  = 1 + hh^2 + kk^2;               % angular momentum magnitude
h   = sqrt( mu * pp );           % angular momentum magnitude
rr  = pp / ( 1 + ecc * cos(nu) );  % radius from the central body

%% Q gradient
dQdCOE = numerical_gradient(sc, st, F, mu, W);

%% g matrix for a, f, g, h, k, L
g(1,1) = (2*aa^2)/h * pp/rr;
g(1,2) = (2*aa^2)/h * ecc*sin(nu);
g(1,3) = 0;
g(2,1) = sqrt(pp/mu)/q * ( (q+1)*cos(LL) + ff );
g(2,2) = sqrt(pp/mu) * sin(LL);
g(2,3) = - sqrt(pp/mu) * gg/q * ( hh*sin(LL) - kk*cos(LL) );
g(3,1) = sqrt(pp/mu)/q * ( (q+1)*sin(LL) + gg );
g(3,2) = - sqrt(pp/mu) * cos(LL);
g(3,3) = sqrt(pp/mu) * ff/q * ( hh*sin(LL) - kk*cos(LL) );
g(4,1) = 0;
g(4,2) = 0;
g(4,3) = sqrt(pp/mu) * s2 * cos(LL) / (2*q);
g(5,1) = 0;
g(5,2) = 0;
g(5,3) = sqrt(pp/mu) * s2 * sin(LL) / (2*q);
g(6,1) = 0;
g(6,2) = 0;
g(6,3) = sqrt(pp/mu) * ( hh*sin(LL) - kk*cos(LL) ) / q;

%% D1, D2, D3 definition
u_opt = - g(1:5,:)' * dQdCOE;
u_opt = real(u_opt);

D1 = u_opt(1);
D2 = u_opt(2);
D3 = u_opt(3);

alpha = atan2(-D2,-D1);
beta  = atan( -D3/sqrt(D1^2+D2^2) );

dQdt = D1*cos(beta)*cos(alpha) + D2*cos(beta)*sin(alpha) + D3*sin(beta);



% Need to search -pi and +pi from current true anomaly to search for min
% and max Qdot

L_range = linspace(LL-pi,LL+pi,100);
Qdot_cutoff_search = zeros(1,length(L_range));
for ii= 1:length(L_range)
    
    % 1. Determine u_opt_temp
    g(1,1) = (2*aa^2)/h * pp/rr;
    g(1,2) = (2*aa^2)/h * ecc*sin(nu);
    g(1,3) = 0;
    g(2,1) = sqrt(pp/mu)/q * ( (q+1)*cos(L_range(ii)) + ff );
    g(2,2) = sqrt(pp/mu) * sin(L_range(ii));
    g(2,3) = - sqrt(pp/mu) * gg/q * ( hh*sin(L_range(ii)) - kk*cos(L_range(ii)) );
    g(3,1) = sqrt(pp/mu)/q * ( (q+1)*sin(L_range(ii)) + gg );
    g(3,2) = - sqrt(pp/mu) * cos(L_range(ii));
    g(3,3) = sqrt(pp/mu) * ff/q * ( hh*sin(L_range(ii)) - kk*cos(L_range(ii)) );
    g(4,1) = 0;
    g(4,2) = 0;
    g(4,3) = sqrt(pp/mu) * s2 * cos(L_range(ii)) / (2*q);
    g(5,1) = 0;
    g(5,2) = 0;
    g(5,3) = sqrt(pp/mu) * s2 * sin(L_range(ii)) / (2*q);
    g(6,1) = 0;
    g(6,2) = 0;
    g(6,3) = sqrt(pp/mu) * ( hh*sin(L_range(ii)) - kk*cos(L_range(ii)) ) / q;
    
    dQdCOE = numerical_gradient_cutoff(sc, st, F, mu, W, L_range(ii));
    
    u_opt_temp = - g(1:6,:)' * dQdCOE;
    u_opt_temp = real(u_opt_temp);

    D1 = u_opt_temp(1);
    D2 = u_opt_temp(2);
    D3 = u_opt_temp(3);
    
    alpha = atan2(-D2,-D1);
    beta  = atan( -D3/sqrt(D1^2+D2^2) );
    
    Qdot_cutoff_search(ii) = D1*cos(beta)*cos(alpha) + D2*cos(beta)*sin(alpha) + D3*sin(beta);
end

Qdotmin = min(Qdot_cutoff_search);
Qdotmax = max(Qdot_cutoff_search);
cutoff_abs = dQdt/Qdotmin;
cutoff_rel = (dQdt-Qdotmax)/(Qdotmin-Qdotmax);











